/*
    Copyright 2021 codenocold codenocold@qq.com
    Address : https://github.com/codenocold/dgm
    This file is part of the dgm firmware.
    The dgm firmware is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.
    The dgm firmware is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.
    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "mc_task.h"
#include "serial.h"
#include "anticogging.h"
#include "calibration.h"
#include "controller.h"
#include "encoder.h"
#include "foc.h"
#include "pwm_curr.h"
#include "usr_config.h"
#include "util.h"
#include <math.h>
#include <string.h>

typedef struct sFSM {
    tFSMState state;
    tFSMState state_next;
    uint8_t state_next_ready;
} tFSM;

static volatile tFSM mFSM;

volatile tMCStatusword StatuswordNew;
volatile tMCStatusword StatuswordOld;


#define CHARGE_BOOT_CAP_MS    10
#define CHARGE_BOOT_CAP_TICKS (uint16_t)((PWM_FREQUENCY * CHARGE_BOOT_CAP_MS) / 1000)
static uint16_t mChargeBootCapDelay = 0;

static void enter_state(void);

static void exit_state(void);

static void led_act_loop(void);

void MCT_init(void) {
    mFSM.state = BOOT_UP;
    mFSM.state_next = BOOT_UP;
    mFSM.state_next_ready = 0;

    StatuswordNew.status.status_code = 0;
    StatuswordNew.errors.errors_code = 0;
    StatuswordOld = StatuswordNew;
}

void MCT_reset_error(void) {
    StatuswordNew.errors.errors_code &= 0x0000FFFF;
    StatuswordOld.errors.errors_code &= 0x0000FFFF;
}

tFSMState MCT_get_state(void) {
    return mFSM.state;
}

// return
//    0 Success
//   -1 Invalid
//   -2 Error code
//   -3 Calib invalid
int MCT_set_state(tFSMState state) {
    int ret = 0;

    switch (mFSM.state) {
        case BOOT_UP:
            if (state == IDLE) {
                mFSM.state_next = IDLE;
            } else {
                ret = -1;
            }
            break;

        case IDLE:
            switch (state) {
                case IDLE:
                    FOC_disarm();
                    mChargeBootCapDelay = 0;
                    mFSM.state_next = IDLE;
                    break;

                case RUN:
                    if (StatuswordNew.errors.errors_code) {
                        ret = -2;
                    } else if (!UsrConfig.calib_valid) {
                        ret = -3;
                    } else {
                        FOC_arm();
                        mChargeBootCapDelay = CHARGE_BOOT_CAP_TICKS;
                        mFSM.state_next = RUN;
                    }
                    break;

                case CALIBRATION:
                    if (StatuswordNew.errors.errors_code) {
                        ret = -2;
                    } else {
                        FOC_arm();
                        mChargeBootCapDelay = CHARGE_BOOT_CAP_TICKS;
                        mFSM.state_next = CALIBRATION;
                    }
                    break;

                case ANTICOGGING:
                    if (StatuswordNew.errors.errors_code) {
                        ret = -2;
                    } else if (!UsrConfig.calib_valid) {
                        ret = -3;
                    } else {
                        FOC_arm();
                        mChargeBootCapDelay = CHARGE_BOOT_CAP_TICKS;
                        mFSM.state_next = ANTICOGGING;
                    }
                    break;

                default:
                    ret = -1;
                    break;
            }
            break;

        default:
            if (state == IDLE) {
                mFSM.state_next = IDLE;
            } else {
                ret = -1;
            }
            break;
    }

    mFSM.state_next_ready = 0;

    return ret;
}

static void enter_state(void) {
    printf("enter_state state = %d\n", mFSM.state);

    switch (mFSM.state) {
        case BOOT_UP:
            break;

        case IDLE:
            break;

        case RUN:
            CONTROLLER_reset();
            StatuswordNew.status.switched_on = 1;
            StatuswordNew.status.target_reached = 1;
            StatuswordNew.status.current_limit_active = 0;
            StatuswordOld.status = StatuswordNew.status;
            break;

        case CALIBRATION:
            CALIBRATION_start();
            break;

        case ANTICOGGING:
            CONTROLLER_reset();
            ANTICOGGING_start();
            break;

        default:
            break;
    }
}

static void exit_state(void) {
    printf("exit_state state = %d\n", mFSM.state);

    switch (mFSM.state) {
        case BOOT_UP:
            CAN_reset_rx_timeout();
            CAN_reset_tx_timeout();
            mFSM.state_next_ready = 1;
            break;

        case IDLE:
            if (mChargeBootCapDelay) {
                mChargeBootCapDelay -= 1;
            } else {
                mFSM.state_next_ready = 1;
            }
            break;

        case RUN:
            FOC_disarm();
            StatuswordNew.status.switched_on = 0;
            StatuswordNew.status.target_reached = 0;
            StatuswordNew.status.current_limit_active = 0;
            StatuswordOld.status = StatuswordNew.status;
            mFSM.state_next_ready = 1;
            break;

        case CALIBRATION:
            CALIBRATION_end();
            mFSM.state_next_ready = 1;
            break;

        case ANTICOGGING:
            ANTICOGGING_end();
            mFSM.state_next_ready = 1;
            break;

        default:
            break;
    }
}

void MCT_high_frequency_task(void) {
    /* state transition management */
    if (mFSM.state_next != mFSM.state) {
        exit_state();
        if (mFSM.state_next_ready) {
            mFSM.state = mFSM.state_next;
            enter_state();
        }
    }

    ENCODER_loop();

    HAL_ADC_Start(&hadc2);
    adc_vbus[0] = HAL_ADC_GetValue(&hadc2);
    Foc.v_bus = read_vbus();
    UTILS_LP_FAST(Foc.v_bus_filt, Foc.v_bus, 0.05f);
    Foc.i_a = read_iphase_a();
    Foc.i_b = read_iphase_b();
    Foc.i_c = read_iphase_c();

    switch (mFSM.state) {
        case BOOT_UP:
            break;

        case CALIBRATION:
            CALIBRATION_loop();
            break;

        case ANTICOGGING:
            ANTICOGGING_loop();

        case RUN: {
            CONTROLLER_loop();

            // check motor current
            // If Ia + Ib + Ic == 0 holds then we have: Inorm^2 = Id^2 + Iq^2 = Ialpha^2 + Ibeta^2 = 2/3 * (Ia^2 + Ib^2 + Ic^2)
            float Inorm_sq = 2.0f / 3.0f * (SQ(Foc.i_a) + SQ(Foc.i_b) + SQ(Foc.i_c));
            if (Inorm_sq > SQ(UsrConfig.protect_over_current)) {
                FOC_disarm();
                MCT_set_state(IDLE);
                StatuswordNew.errors.over_current = 1;
            }

            // check I bus current
            if (Foc.i_bus > UsrConfig.protect_i_bus_max) {
                FOC_disarm();
                MCT_set_state(IDLE);
                StatuswordNew.errors.over_current = 1;
            }
        }
            break;

        default:
            break;
    }
}

void MCT_safety_task(void) {
    // VBUS check
    if (mFSM.state != BOOT_UP) {
        // Over voltage check
        if (Foc.v_bus > UsrConfig.protect_over_voltage) {
            StatuswordNew.errors.over_voltage = 1;
        }

        // Under voltage check
        if (Foc.v_bus < UsrConfig.protect_under_voltage) {
            StatuswordNew.errors.under_voltage = 1;
        }

        // Enchoder state check
        if (MA600.check_err_count > 50 || MA600.rx_err_count > 50) {
            StatuswordNew.errors.encoder_offline = 1;
        }
    }

    watch_dog_feed();
}

void MCT_low_priority_task(void) {
    bool isSend = false;

    // State check
    if (StatuswordOld.status.status_code != StatuswordNew.status.status_code) {
        isSend = true;
        StatuswordOld.status.status_code = StatuswordNew.status.status_code;
    }

    // Error check
    if (StatuswordOld.errors.errors_code != StatuswordNew.errors.errors_code) {
        if (StatuswordNew.errors.errors_code) {
            FOC_disarm();
            MCT_set_state(IDLE);
        }

        isSend = true;
        StatuswordOld.errors.errors_code = StatuswordNew.errors.errors_code;
    }

    if (isSend) {
        CAN_tx_statusword(StatuswordNew);
    }

    led_act_loop();
    CAN_comm_loop();
}

static void led_act_loop(void) {
    static uint16_t tick = 0;
    static uint32_t tick_100Hz = 0;

    // 100Hz
    if (get_ms_since(tick_100Hz) < 10) {
        return;
    }
    tick_100Hz = SystickCount;

    switch (mFSM.state) {
        case IDLE:
            if (tick == 0) {
                LED_ACT_SET();
            } else if (tick == 10) {
                LED_ACT_RESET();
            } else if (tick > 100) {
                tick = 0xFFFF;
            }
            break;

        case RUN:
            if (tick == 0) {
                LED_ACT_SET();
            } else if (tick == 10) {
                LED_ACT_RESET();
            } else if (tick == 20) {
                LED_ACT_SET();
            } else if (tick == 30) {
                LED_ACT_RESET();
            } else if (tick > 100) {
                tick = 0xFFFF;
            }
            break;

        case CALIBRATION:
            if (tick == 0) {
                LED_ACT_SET();
            } else if (tick == 10) {
                LED_ACT_RESET();
            } else if (tick == 20) {
                LED_ACT_SET();
            } else if (tick == 30) {
                LED_ACT_RESET();
            } else if (tick == 40) {
                LED_ACT_SET();
            } else if (tick == 50) {
                LED_ACT_RESET();
            } else if (tick > 150) {
                tick = 0xFFFF;
            }
            break;

        case ANTICOGGING:
            if (tick == 0) {
                LED_ACT_SET();
            } else if (tick == 10) {
                LED_ACT_RESET();
            } else if (tick == 20) {
                LED_ACT_SET();
            } else if (tick == 30) {
                LED_ACT_RESET();
            } else if (tick == 40) {
                LED_ACT_SET();
            } else if (tick == 50) {
                LED_ACT_RESET();
            } else if (tick == 60) {
                LED_ACT_SET();
            } else if (tick == 70) {
                LED_ACT_RESET();
            } else if (tick > 200) {
                tick = 0xFFFF;
            }
            break;

        default:
            break;
    }

    tick++;
}
